package com.awesumgames.awesum.physics;

import javax.vecmath.Matrix4f;
import javax.vecmath.Quat4f;

import com.awesumgames.awesum.Drawable;
import com.awesumgames.awesum.Vector3;
import com.awesumgames.awesum.AwesumGlobal.Awesum;
import com.bulletphysics.collision.broadphase.*;
import com.bulletphysics.collision.narrowphase.*;
import com.bulletphysics.collision.dispatch.*;
import com.bulletphysics.collision.shapes.*;
import com.bulletphysics.dynamics.*;
import com.bulletphysics.dynamics.constraintsolver.*;
import com.bulletphysics.linearmath.*;

public final class Physics {
	
	public DiscreteDynamicsWorld world = null;
	
	public void load() {
		BroadphaseInterface broadphase = new DbvtBroadphase();
		CollisionConfiguration collConfig = new DefaultCollisionConfiguration();
		CollisionDispatcher collDispatcher = new CollisionDispatcher(collConfig);
		ConstraintSolver cs = new SequentialImpulseConstraintSolver();
		world = new DiscreteDynamicsWorld(collDispatcher, broadphase, cs, collConfig);
		world.setGravity(new Vector3(0f, -20f, 0f));
	}

	public void step(float seconds) {
		if (world != null) {
			world.stepSimulation(seconds, 10);
			
			int colls = world.getDispatcher().getNumManifolds();
			//Awesum.debug("physics world step colls = " + colls);
			for (int i = 0; i < colls; i++) {
				PersistentManifold pm = world.getDispatcher().getManifoldByIndexInternal(i);
				Drawable d1 = (Drawable)((CollisionObject)pm.getBody0()).getUserPointer();
				Drawable d2 = (Drawable)((CollisionObject)pm.getBody1()).getUserPointer();
				
				int contacts = pm.getNumContacts();
				//if (contacts > 0)
					//Awesum.debug("contact points = " + contacts);
				for (int j = 0; j < contacts; j++) {
					ManifoldPoint p = pm.getContactPoint(j);
					if (p.getDistance() < 0f) {
						Vector3 p1 = (Vector3) p.getPositionWorldOnA(new Vector3());
						Vector3 p2 = (Vector3) p.getPositionWorldOnB(new Vector3());
						
						if (d1 != null && d1.phys.receiveCollisions)
							d1.collide(d2, p1, p2, new Vector3(p.normalWorldOnB));
						if (d2 != null && d2.phys.receiveCollisions)
							d2.collide(d1, p2, p1, new Vector3(p.normalWorldOnB).negate2());
					}
				}
			}
		}
	}
	
	public void addGround(float yAxis) {
		CollisionShape ground = new StaticPlaneShape(new Vector3(0f, 1f, 0f), .25f);
		MotionState ms = new DefaultMotionState(new Transform(new Matrix4f(new Quat4f(0f, 0f, 0f, 1f), new Vector3(0f, yAxis, 0f), 1f)));
		RigidBodyConstructionInfo rbci = new RigidBodyConstructionInfo(0f, ms, ground, new Vector3(0f, 0f, 0f));
		rbci.restitution = 1f;
		RigidBody body = new RigidBody(rbci);
		world.addRigidBody(body);
	}
	
	public void addPlane(Vector3 pos, Vector3 normal) {
		CollisionShape plane = new StaticPlaneShape(normal.copy(), .25f);
		MotionState ms = new DefaultMotionState(new Transform(new Matrix4f(new Quat4f(0f, 0f, 0f, 1f), pos.copy(), 1f)));
		RigidBodyConstructionInfo rbci = new RigidBodyConstructionInfo(0f, ms, plane, new Vector3(0f, 0f, 0f));
		rbci.restitution = 1f;
		RigidBody body = new RigidBody(rbci);
		world.addRigidBody(body);
	}

}
